#ifndef ROBOTJOINTGROUP_H
#define ROBOTJOINTGROUP_H

#include <QMap>

#include "robotcomponent.h"

/**
 * @brief A RobotComponent that contains one or more joints.
 *
 * It makes sense to group joints into one component.  For example, with
 * our Troy model, the shoulder is represented by one physical "joint", but
 * that one joint is actually made up of three joints that can rotate about
 * different axes.  This allows us to have fine control over the rotation
 * that occurs at one joint "group".
*/
class RobotJointGroup : public RobotComponent
{
    Q_OBJECT

private:
    /**
     * @brief Represents a joint of the robot, with an axis of rotation,
     * minimum, and maximum angles.
     *
    */
    typedef struct robotJoint{
        int id;
        float min;
        float max;
        float curAngle;
        QVector3D rotationAxis;
        QString* description;
    } RobotJoint;

    /**
     * @brief A map of joint IDs to the joints contained by this group.
    */
    QMap<int, RobotJoint*> _joints;

    /**
     * @brief Helper function used by SetJointAngle and MoveJointBy for code reuse
     *
     * @param joint The RobotJoint we are changing.
     * @param theta The absolute angle (in degrees) at which to set the joint.
     * @return float The absolute angle (in degrees) at which the joint was set.
    */
    float setAngle(RobotJoint* joint, float theta);

public:
    /**
     * @brief Constructor.  Creates a new RobotJointGroup.
     *
     * @param names An array of unsigned ints that represent a unique name for this joint group.
     * @param description A human-readable description of this group.
     * @param parent The parent RobotComponent of this group.
     * @param radius The radius of this joint group.
     * @param offset The offset (in 3D space) of the joint's start point from its parent's end point.
     * @param invertLength True if length should be measured in the +y direction.
    */
    explicit RobotJointGroup(NameGroup names, QString description = "",
                             RobotComponent *parent = 0, float radius = JOINT_RADIUS,
                             QVector3D offset = QVector3D(0,0,0), bool invertLength = false);
    /**
     * @brief Destructor.  Destroys this joint group and frees any dynamic memory used.
     *
    */
    ~RobotJointGroup();

    //Functions for creating and removing joints
    /**
     * @brief Adds a new joint to this group.
     *
     * @param id The unique ID of the joint.
     * @param min The minimum angle (in degrees) of the joint.
     * @param max The maximum angle (in degrees) of the joint.
     * @param rotationAxis The 3D axis that the joint rotates around.
     * @param description A human-readable description of the joint.
    */
    void addJoint(int id, float min, float max, QVector3D rotationAxis, QString description = "");
    /**
     * @brief Removes a joint from this group.
     *
     * @param jointID The joint ID of the joint to remove
    */
    void removeJoint(int jointID);

    //Functions for manipulating the joints
    /**
     * @brief Sets a joint's angle to an absolute angle.  The angle is corrected for
     * minimum and maximum values.
     *
     * @param jointID The joint ID of the joint to change.
     * @param theta The angle (in degrees) at which to set the joint.
     * @return float The angle (in degrees) at which the joint was set.
    */
    float setJointAngle(int jointID, float theta);
    /**
     * @brief Moves a joint's angle by a given amount.  The angle is corrected for
     * minimum and maximum values.
     *
     * @param jointID The joint ID of the joint to change.
     * @param delta The amount (in degrees) to move the joint's angle.
     * @return float The absolute angle (in degrees) at which the joint was set.
    */
    float moveJointBy(int jointID, float delta);

    //Functions for information about the joints
    /**
     * @brief Returns a list of joint IDs that this group contains.
     *
     * @return const QList<int> The list of joint IDs contained by this joint group.
    */
    const QList<int> jointIDs() const;
    /**
     * @brief Returns true if the group contains a joint.
     *
     * @param jointID The ID of the joint to find.
     * @return bool True if the group contains the joint; otherwise false.
    */
    bool containsJoint(int jointID) const;
    /**
     * @brief Returns the current angle of a joint.
     *
     * @param jointID The ID of the joint.
     * @return float The angle of the joint, if found; otherwise returns INVALID_JOINT.
    */
    float jointAngle(int jointID) const;
    /**
     * @brief Returns the human-readable description of a joint.
     *
     * @param jointID The ID of the joint.
     * @return QString A human-readable description of the joint, if found.  Otherwise,
     * returns a null QString.
    */
    QString jointDescription(int jointID) const;
    /**
     * @brief Returns the maximum angle of a joint.
     *
     * @param jointID The ID of the joint.
     * @return float The maximum angle of the joint, if found; otherwise returns INVALID_JOINT.
    */
    float minimum(int jointID) const;
    /**
     * @brief Returns the maximum angle of a joint.
     *
     * @param jointID The ID of the joint.
     * @return float The minimum angle of the joint, if found; otherwise returns INVALID_JOINT.
    */
    float maximum(int jointID) const;

    //Returns the total rotation of this joint as a normalized quaternion
    /**
     * @brief Returns the total rotation of this joint as a normalized quaternion.
     *
     * A quaternion is a convenient way of storing 3D rotations.  The math is a little
     * complex (pun intended), but Qt has a good libaries that deal with quaternions.
     *
     * @return QQuaternion A normalized quaternion representing the total rotation of this
     * joint group.
    */
    QQuaternion rotation() const;
};

#endif // ROBOTJOINTGROUP_H
